#include "UKF.h"

using namespace std;
using namespace ukf;

int main(int argc, char **argv)
{   
    //ColumnVector stateX(3); //3个状态值 
    ColumnVector measurementZ(4); //4个测量值
    float deltaT = 0.5;//s

    UKF ukf(3/*状态值个数*/,4/*测量值个数*/);

    
    //为了模拟测试，我们需要利用飞机的运动学模型生成一些模拟的测量数据,同时模拟传感器误差：测量值列向量Z_k: (d_gps,h_gps,p_bp,v_speed)  m=4
    //ukf.GenarateDemoMeasurementData();
    /*  我的一次运行结果的测量值
    GenarateDemoMeasurementData: d_measurement= [45.476,92.282,101.418,175.885,205.681,248.808,316.264,374.051,460.168,506.615,586.392,689.5,764.937,849.705,939.803,1054.23,1166.99,1234.08,1369.49,1458.24,1584.32,1706.73,1828.47,2000.54,2126.94,2236.67,2413.73,2573.11,2698.83,2872.88,3030.26,3213.97,3349.01,3516.38,3729.08,3869.11,4082.47,4250.17,4481.19,4650.54,4911.22,5091.23,5292.57,5542.24,5765.24,5997.58,6212.24,6451.23,6668.55,6925.2,7203.19,7453.5,7684.14,7941.11,8230.42,8507.05,8758.01,9027.31,9336.93,9597.88,]
    GenarateDemoMeasurementData: h_measurement= [34.75,65,62.75,71,112.75,142,203.75,245,240.75,284,346.75,397,408.75,470,563.75,615,653.75,703,798.75,864,936.75,966,1044.75,1158,1212.75,1290,1362.75,1458,1538.75,1621,1734.75,1829,1931.75,2043,2120.75,2271,2387.75,2475,2560.75,2721,2790.75,2960,3059.75,3216,3293.75,3439,3600.75,3745,3845.75,4021,4139.75,4298,4466.75,4617,4740.75,4929,5028.75,5236,5397.75,5546,]
    GenarateDemoMeasurementData: h_measurement_barometers= [-27.25,-15,62.75,71,112.75,142,133.75,165,240.75,284,310.75,353,408.75,470,493.75,555,653.75,703,738.75,806,870.75,966,1044.75,1092,1212.75,1290,1362.75,1458,1538.75,1621,1734.75,1829,1931.75,2007,2120.75,2199,2299.75,2435,2560.75,2649,2790.75,2890,3059.75,3134,3293.75,3439,3536.75,3665,3845.75,3949,4139.75,4252,4380.75,4533,4740.75,4841,5028.75,5174,5339.75,5546,]
    GenarateDemoMeasurementData: v_measurement= [79,90,99,111,121,127,139,150,157,171,182,188,201,207,219,227,237,247,257,268,278,292,300,311,322,331,338,348,359,372,379,392,401,412,418,432,440,447,459,472,478,491,501,507,521,531,539,549,558,567,579,591,600,610,622,632,642,648,657,672,]
    GenarateDemoMeasurementData: bp_measurement_barometers= [101.653,101.505,100.573,100.474,99.9769,99.6295,99.7274,99.3571,98.464,97.9571,97.6446,97.1527,96.5066,95.8009,95.5283,94.8284,93.7087,93.1542,92.7534,92.0033,91.2857,90.2383,89.3797,88.8677,87.5701,86.748,85.9796,84.9818,84.1433,83.2961,82.1358,81.1844,80.1574,79.4119,78.2958,77.5353,76.565,75.278,74.0971,73.2774,71.9761,71.0761,69.558,68.9023,67.5085,66.2611,65.4322,64.3574,62.8669,62.0282,60.5024,59.6188,58.6183,57.4527,55.8926,55.1521,53.7868,52.7493,51.5854,50.1661,]
    */

    double d_measurement[60]= {45.476,92.282,101.418,175.885,205.681,248.808,316.264,374.051,460.168,506.615,586.392,689.5,764.937,849.705,939.803,1054.23,1166.99,1234.08,1369.49,1458.24,1584.32,1706.73,1828.47,2000.54,2126.94,2236.67,2413.73,2573.11,2698.83,2872.88,3030.26,3213.97,3349.01,3516.38,3729.08,3869.11,4082.47,4250.17,4481.19,4650.54,4911.22,5091.23,5292.57,5542.24,5765.24,5997.58,6212.24,6451.23,6668.55,6925.2,7203.19,7453.5,7684.14,7941.11,8230.42,8507.05,8758.01,9027.31,9336.93,9597.88};
    double h_measurement[60]= {34.75,65,62.75,71,112.75,142,203.75,245,240.75,284,346.75,397,408.75,470,563.75,615,653.75,703,798.75,864,936.75,966,1044.75,1158,1212.75,1290,1362.75,1458,1538.75,1621,1734.75,1829,1931.75,2043,2120.75,2271,2387.75,2475,2560.75,2721,2790.75,2960,3059.75,3216,3293.75,3439,3600.75,3745,3845.75,4021,4139.75,4298,4466.75,4617,4740.75,4929,5028.75,5236,5397.75,5546};
    //double h_measurement_barometers[60]= {0,0,62.75,71,112.75,142,133.75,165,240.75,284,310.75,353,408.75,470,493.75,555,653.75,703,738.75,806,870.75,966,1044.75,1092,1212.75,1290,1362.75,1458,1538.75,1621,1734.75,1829,1931.75,2007,2120.75,2199,2299.75,2435,2560.75,2649,2790.75,2890,3059.75,3134,3293.75,3439,3536.75,3665,3845.75,3949,4139.75,4252,4380.75,4533,4740.75,4841,5028.75,5174,5339.75,5546};
    double bp_measurement_barometers[60]= {101.325,101.325,100.573,100.474,99.9769,99.6295,99.7274,99.3571,98.464,97.9571,97.6446,97.1527,96.5066,95.8009,95.5283,94.8284,93.7087,93.1542,92.7534,92.0033,91.2857,90.2383,89.3797,88.8677,87.5701,86.748,85.9796,84.9818,84.1433,83.2961,82.1358,81.1844,80.1574,79.4119,78.2958,77.5353,76.565,75.278,74.0971,73.2774,71.9761,71.0761,69.558,68.9023,67.5085,66.2611,65.4322,64.3574,62.8669,62.0282,60.5024,59.6188,58.6183,57.4527,55.8926,55.1521,53.7868,52.7493,51.5854,50.1661};
    double v_measurement[60]= {79,90,99,111,121,127,139,150,157,171,182,188,201,207,219,227,237,247,257,268,278,292,300,311,322,331,338,348,359,372,379,392,401,412,418,432,440,447,459,472,478,491,501,507,521,531,539,549,558,567,579,591,600,610,622,632,642,648,657,672};
    
    //保存结果最优估计值X_post数组
    ColumnVector statePostEstimateArray[60];
    
    //我根据生成模拟测量值数据时,设置的噪音,大致设置下各传感器噪音的方差(协方差).  
    //实际项目中应从传感器说明手册或厂家获取.
    SymmetricMatrix R(4); 
    R = 0.0;
    R(1, 1) = 0.0000001; // GPS噪音方差(协方差) -> d
    R(2, 2) = 0.0000001; // GPS噪音方差(协方差) -> h
    R(3, 3) = 0.00001;     // 气压计噪音噪音方差(协方差) -> h
    R(4, 4) = 0.00000001; // 速度计噪音噪音方差(协方差)  -> v

    ukf.UpdateRK(R);//更新下传感器噪音

    for( int i = 0; i < 60; i++ ) // 模拟预测和更新周期是2， 即每秒2执行2次。  30秒执行60次.  deltaT = 0.5s,当然这里不用真去delay(500)
    {
        measurementZ(1) = d_measurement[i];
        measurementZ(2) = h_measurement[i];
        measurementZ(3) = bp_measurement_barometers[i];
        measurementZ(4) = v_measurement[i];

        ukf.PredictAndUpdate(deltaT, measurementZ);

        ukf.GetPostEstimate(statePostEstimateArray[i] /*out*/);
    }

    cout << "statePostEstimateArray=\n[\n";
    for (unsigned int i = 0; i < 60; i++)
    {
        cout << statePostEstimateArray[i] << ",";
    }
    cout << "]" << endl;

    cout << "state.d=\n[\n";
    for (unsigned int i = 0; i < 60; i++)
    {
        cout << statePostEstimateArray[i](1) << ",";
    }
    cout << "]" << endl;

    cout << "state.h=\n[\n";
    for (unsigned int i = 0; i < 60; i++)
    {
        cout << statePostEstimateArray[i](2) << ",";
    }
    cout << "]" << endl;

    cout << "state.v=\n[\n";
    for (unsigned int i = 0; i < 60; i++)
    {
        cout << statePostEstimateArray[i](3) << ",";
    }
    cout << "]" << endl;
    

    return 0;
}


//某次运行,输出结果如下, 我也根据下列数据绘制excel图表
/*
statePostEstimateArray=
[ [3](45.476,34.7499,79),[3](92.2196,64.9602,90.0078),[3](101.401,62.7449,99.0048),[3](175.705,70.922,111.021),[3](205.51,112.616,121.025),[3](248.564,141.84,127.033),[3](315.889,203.489,139.049),[3](373.628,244.723,150.057),[3](459.638,240.468,157.067),[3](506.08,283.642,171.073),[3](585.693,346.289,182.092),[3](688.638,396.463,188.112),[3](764.071,408.23,201.114),[3](848.685,469.339,207.135),[3](938.637,562.968,219.155),[3](1052.9,614.172,227.174),[3](1165.55,652.86,237.189),[3](1232.62,702.057,247.197),[3](1367.73,797.613,257.231),[3](1456.44,862.842,268.24),[3](1582.3,935.469,278.266),[3](1704.62,964.697,292.278),[3](1826.17,1043.28,300.304),[3](1997.93,1156.33,311.342),[3](2124.27,1211.07,322.353),[3](2233.87,1288.2,331.373),[3](2410.61,1360.8,338.408),[3](2569.81,1455.9,348.435),[3](2695.42,1536.57,359.452),[3](2869.19,1618.68,372.485),[3](3026.36,1732.25,379.516),[3](3209.82,1826.37,392.547),[3](3344.74,1929.01,401.568),[3](3511.84,2040.1,412.601),[3](3724.22,2117.71,418.638),[3](3864.13,2267.76,432.664),[3](4077.11,2384.35,440.706),[3](4244.67,2471.51,447.728),[3](4475.33,2557.08,459.77),[3](4644.49,2717.09,472.803),[3](4904.76,2786.73,478.846),[3](5084.6,2955.72,491.88),[3](5285.68,3055.38,501.911),[3](5534.91,3211.33,507.966),[3](5757.73,3289.03,521.99),[3](5989.72,3433.99,532.038),[3](6204.09,3595.53,540.079),[3](6442.74,3739.6,550.122),[3](6659.83,3840.23,559.152),[3](6916.04,4015.15,568.209),[3](7193.69,4133.75,580.252),[3](7443.69,4291.75,592.296),[3](7674.03,4460.29,601.339),[3](7930.63,4610.34,611.385),[3](8219.55,4733.88,623.433),[3](8495.79,4921.82,633.487),[3](8746.52,5021.49,643.519),[3](9015.36,5228.35,649.581),[3](9324.54,5389.89,658.634),[3](9585.24,5537.96,673.672),]
state.d=
[ 45.476,92.2196,101.401,175.705,205.51,248.564,315.889,373.628,459.638,506.08,585.693,688.638,764.071,848.685,938.637,1052.9,1165.55,1232.62,1367.73,1456.44,1582.3,1704.62,1826.17,1997.93,2124.27,2233.87,2410.61,2569.81,2695.42,2869.19,3026.36,3209.82,3344.74,3511.84,3724.22,3864.13,4077.11,4244.67,4475.33,4644.49,4904.76,5084.6,5285.68,5534.91,5757.73,5989.72,6204.09,6442.74,6659.83,6916.04,7193.69,7443.69,7674.03,7930.63,8219.55,8495.79,8746.52,9015.36,9324.54,9585.24,]
state.h=
[ 34.7499,64.9602,62.7449,70.922,112.616,141.84,203.489,244.723,240.468,283.642,346.289,396.463,408.23,469.339,562.968,614.172,652.86,702.057,797.613,862.842,935.469,964.697,1043.28,1156.33,1211.07,1288.2,1360.8,1455.9,1536.57,1618.68,1732.25,1826.37,1929.01,2040.1,2117.71,2267.76,2384.35,2471.51,2557.08,2717.09,2786.73,2955.72,3055.38,3211.33,3289.03,3433.99,3595.53,3739.6,3840.23,4015.15,4133.75,4291.75,4460.29,4610.34,4733.88,4921.82,5021.49,5228.35,5389.89,5537.96,]
state.v=
[ 79,90.0078,99.0048,111.021,121.025,127.033,139.049,150.057,157.067,171.073,182.092,188.112,201.114,207.135,219.155,227.174,237.189,247.197,257.231,268.24,278.266,292.278,300.304,311.342,322.353,331.373,338.408,348.435,359.452,372.485,379.516,392.547,401.568,412.601,418.638,432.664,440.706,447.728,459.77,472.803,478.846,491.88,501.911,507.966,521.99,532.038,540.079,550.122,559.152,568.209,580.252,592.296,601.339,611.385,623.433,633.487,643.519,649.581,658.634,673.672,]
*/